/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "World.h"

#include "GL/gl.h"
#include "GL/glu.h"

#include "Material.h"
//#include "MaterialHandler.h"
#include "Object.h"
#include "Primitive.h"
#include "Terrain.h"
#include "TestObject.h"
#include "LaserScanner.h"
#include "J2B2.h"
#include "settings.h"

#include "drawutils.h"
#include "mathutils.h"

#define GIMI_CLIENT_API 20000
#include "gimi.h"

#include "owndebug.h"

GLfloat light_ambient[] = { 0.5, 0.5, 0.5, 1.0 };
GLfloat light_diffuse[] = { 0.6, 0.6, 0.6, 1.0 };
GLfloat light_specular[] = { 0.0, 0.0, 0.0, 1.0 };
GLfloat light_position[] = { 15.0, 15.0, 10.0, 0.0 };

GLfloat light_center_position[] = { 0.0, 15.0, 0.0, 0.0 };



CWorld::CWorld()
  :SimulationTime(0),
   StepSize(0.02),
   QuickStepIters(40),
   QuickMode(false),
   GLProvider(NULL),
   graphicsInited(false),
   useGimiSettingsFile(true),
   gimiSettingsFile("gimi_settings.xml"),
   hubAddress(""),
   hubPort(-1),
   iMCS(),
   iLineMapServer(NULL),
   iLineMapServerConstructed(false),
   iDrawGridMode(true),
   iMakeNewWallDisplayList(true),
   iWallDisplayListID(),
   iTotalWalls(0)

{

  dInitODE();
  
  dAllocateODEDataForThread(0);
  
  // OpenGLTextureLoader
  iOpenGLTextureLoader = new COpenGLTextureLoader();

  //MaterialHandler=new CMaterialHandler;
  SpaceID=dSimpleSpaceCreate(0);
  StaticSpaceID=dSimpleSpaceCreate(0);
  WorldID=dWorldCreate();
  contactgroup = dJointGroupCreate(0);
  dWorldSetGravity(WorldID, 0,-9.8, 0);
  dWorldSetERP(WorldID, 0.8);
  dWorldSetCFM(WorldID, 1e-3);
  dWorldSetContactMaxCorrectingVel(WorldID, dInfinity); //was 1 previously
  dWorldSetContactSurfaceLayer(WorldID, 0.0001); //0.0001
  dWorldSetAutoDisableFlag(WorldID, 0);
  dWorldSetQuickStepNumIterations (WorldID, QuickStepIters);

  //InsertObject(new CTerrain(this));

}

CWorld::~CWorld()
{
  //dPrint(1,"deleting");
  ObjectsIter iter;

  for (iter=Objects.begin(); iter!=Objects.end(); iter++)
  {
    //Objects.erase(iter);
    delete (CObject*)*iter;
  }

  dJointGroupDestroy(this->contactgroup);
  dSpaceDestroy(this->SpaceID);
  dSpaceDestroy(this->StaticSpaceID);
  dWorldDestroy(this->WorldID);

}

void CWorld::InsertObject(CObject* object)
{
  
  ObjectsToAdd.push_back(object);

}

void CWorld::SetBaseMaCIGroup(std::string &aGroupName)
{
  iBaseMaCIGroup = aGroupName;

}

std::string CWorld::GetBaseMaCIGroup(){

  return iBaseMaCIGroup;
}

void CWorld::AddObjects()
{
  if(iLineMapServerConstructed &&iLineMapServer->IsActive() && ObjectsToAdd.size() > 0){
    std::vector<CWall *> walls;
    for(std::list<CObject *>::size_type i = 0; i < ObjectsToAdd.size(); i++){
      CWall *wall = dynamic_cast<CWall *>(ObjectsToAdd[i]);
      if(wall){
        walls.push_back(wall);
      }
    }
    UpdateMapServer(walls, true);    
  }
  Objects.insert(Objects.end(),ObjectsToAdd.begin(),ObjectsToAdd.end());
  ObjectsToAdd.clear();
}

bool CWorld::RemoveObject(CObject* object)
{
  ObjectsIter iter;
  
  for (iter=Objects.begin(); iter!=Objects.end(); iter++)
  {
    if (*iter==object)
    {
      ObjectsToRemove.push_back(object);
      return true;
    }
  }
  return false;
}

void CWorld::SetTerrain(CTerrain *terrain)
{
  this->terrain=terrain;
  InsertObject(terrain);
}

void CWorld::RemoveObjects()
{
  ObjectsIter iter,iter2;
  
  for (iter=ObjectsToRemove.begin(); iter!=ObjectsToRemove.end(); iter++)
  {
    for (iter2=Objects.begin(); iter2!=Objects.end(); iter2++)
    {
      if (*iter==*iter2)
      {
         Objects.erase(iter2);
         break;
      }
    }
  }
  ObjectsToRemove.clear();
}

void CWorld::Activate()
{

  CreateMapServer();
  std::vector<CWall *> walls;
  for(std::list<CObject *>::size_type i = 0; i < Objects.size(); i++){
    CWall *wall = dynamic_cast<CWall *>(Objects[i]);
    if(wall){
      walls.push_back(wall);
              
    }
  }
  UpdateMapServer(walls, true);
  ObjectsIter iter;
  AddObjects();
  
  for (iter=Objects.begin(); iter!=Objects.end(); iter++){
    //Objects.erase(iter);
    (*iter)->Activate();;
  }
  
  

  
}

//takes simulation step
void CWorld::Step()
{
  //dPrint(1,"step");
  //inform objects that step is taken
  UpdateObjects();
  //find collisions in worlds main space
  dSpaceCollide(SpaceID, (void*)this, &NearCallback); ///< good
  //dSpaceCollide(StaticSpaceID, (void*)this, &NearCallback);
  //take step

  //dPrint(0, "SPACEID HAS %d GEOMS!", dSpaceGetNumGeoms(SpaceID));
  //dPrint(0, "STATICSPACEID HAS %d GEOMS!", dSpaceGetNumGeoms(StaticSpaceID));

  for(int i = 0; i <  dSpaceGetNumGeoms(SpaceID); i++)
    {
      for(int j = 0; j <  dSpaceGetNumGeoms(StaticSpaceID); j++)
        {
          dSpaceCollide2(dSpaceGetGeom(SpaceID, i), dSpaceGetGeom(StaticSpaceID, j), (void*)this, &NearCallback);
        }
    }


  //dSpaceCollide2(SpaceID, StaticSpaceID, (void*)this, &NearCallback);

  if (this->QuickMode)
  {
    dWorldQuickStep(WorldID, this->StepSize);
  } else {
    dWorldStep(WorldID, this->StepSize);
  }
  //discard temporary joints
  dJointGroupEmpty(this->contactgroup);

  this->SimulationTime+=this->StepSize;
  
  AddObjects();
  RemoveObjects();
  
}

void CWorld::InitGraphics()
{
  printf("World.cpp GL INIT GRAPHICS CALLED!\n");
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_CULL_FACE);
  //glEnable(GL_COLOR_MATERIAL);
  glEnable(GL_LIGHTING);
  glEnable(GL_LIGHT0);

  glShadeModel(GL_SMOOTH);
  glDepthFunc(GL_LEQUAL);
  glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);
  
  glCullFace(GL_BACK);

  glClearColor(0.0, 0.0, 0.0, 0.0);

  glLightfv(GL_LIGHT0, GL_AMBIENT, light_ambient);
  glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse);
  glLightfv(GL_LIGHT0, GL_SPECULAR, light_specular);
}


void CWorld::Draw()
{
  //dPrint(1,"draw");
  
/*  glBegin(GL_LINES);
    //x
    glColor3f(1,0,0);
    glVertex3f(0,0,0);
    glVertex3f(1,0,0);
    //z
    glColor3f(0,0,1);
    glVertex3f(0,0,0);
    glVertex3f(0,1,0);
    //y
    glColor3f(0,1,0);
    glVertex3f(0,0,0);
    glVertex3f(0,0,-1);
  glEnd();*/
  

  if (!graphicsInited)
  {
    InitGraphics();
    graphicsInited=true;
  }


  glLightfv(GL_LIGHT0, GL_POSITION, light_center_position);


  int totalWalls = 0;

  ObjectsIter iter;
  // We draw all objects except walls. We count the number of walls.
  for (iter=Objects.begin(); iter!=Objects.end(); iter++)
  {
    
    CWall *wall = dynamic_cast<CWall *>(*iter);
    if(wall){
      totalWalls++;
      continue;
    }

    (*iter)->Draw();
  }

  // If we need to create a new display list ID for all the walls
  if(iMakeNewWallDisplayList == true || totalWalls != iTotalWalls)
    {
      dPrint(0, "Creating display list for walls");
      // We initialize the display list
      iWallDisplayListID = glGenLists(1);

      if(iWallDisplayListID == 0)
        {
          dPrint(0, "FAILED TO CREATE DISPLAY LIST, ABORTING!\n");
          abort();
          
        }

      glNewList(iWallDisplayListID,GL_COMPILE);


      // We iterate through all our objects
      for (iter=Objects.begin(); iter!=Objects.end(); iter++)
        {
          // We use dynamic cast to check for wall
          CWall *wall = dynamic_cast<CWall *>(*iter);
          if(wall)
            {
              (*iter)->Draw();
            
            }
        }

      // endList
      glEndList();
      
      // We no longer need to make a display list
      iMakeNewWallDisplayList = false;
      iTotalWalls = totalWalls;
    }
  

  // We call the wall display list
  glCallList(iWallDisplayListID);


  //dPrint(0, "We have %d walls.", totalWalls);



  //dPrint(0, "Draw end!");

}

void CWorld::SetStepSize(float StepSize)
{
  if (StepSize>0)
    this->StepSize=StepSize;
}

float CWorld::GetStepSize()
{
  return this->StepSize;
}

float CWorld::GetSimulationTime()
{
  return this->SimulationTime;
}

void CWorld::SetQuickMode(bool QuickMode)
{
  this->QuickMode=QuickMode;
}

bool CWorld::GetQuickMode()
{
  return QuickMode;
}

CGLProvider* CWorld::GetGLProvider()
{
  return GLProvider;
}

void CWorld::SetGLProvider(CGLProvider* GLProvider)
{
  this->GLProvider=GLProvider;
}

void CWorld::UpdateObjects()
{
  //iterate through all objects and call update
  ObjectsIter iter;

  for (iter=Objects.begin(); iter!=Objects.end(); iter++)
  {
    (*iter)->Update(this->StepSize);
  }
}

//a new, bit more efficient collision callback :) (compare proto)
void CWorld::NearCallback(void *data, dGeomID o1, dGeomID o2)
{
  //dPrint(1,"callback");
  dContact contacts[MAX_CONTACTS];

  int numc;

  //data given to this callback should be pointer to world that is handled
  CWorld *W=(CWorld*)data;

  //primitives that these geoms belong to should be set as their data
  TGeomData *gd1=(TGeomData*)dGeomGetData(o1);
  TGeomData *gd2=(TGeomData*)dGeomGetData(o2);
  CPrimitive *p1=NULL;
  CPrimitive *p2=NULL;
  if (gd1!=NULL)
    p1=gd1->Primitive;
  if (gd2!=NULL)
    p2=gd2->Primitive;

  dSurfaceParameters s;
  dBodyID b1 = dGeomGetBody(o1);
  dBodyID b2 = dGeomGetBody(o2);
  
  //if both geoms are static, eg. pieces of ground model, or if they belong to 
  //the same body, no need to check collisions
  //IMPORTANT: check these before calculating collisions with dCollide! saves a 
  //lot of time...
  if (p1->IsStatic && p2->IsStatic)
    return;
  
  if (b1 == b2)
    return;
  
  //find out the number of contacts between geoms
  if ((numc = dCollide(o1, o2, MAX_CONTACTS, &contacts[0].geom, sizeof(dContact))))
  {
    

    //get contact defined by these geom's materials
    //s=W->MaterialHandler->GetContact(o1,o2);
    s=W->getContact(o1,o2);
    
    for (int i = 0; i < numc; i++)
    {
      //create contacts and attach them
      contacts[i].surface=s;

      dJointID j = dJointCreateContact(W->WorldID, W->contactgroup, contacts + i);
      //do not create contact if other geom is ray
      if ((dGeomGetClass(contacts[0].geom.g1)!= dRayClass) && (dGeomGetClass(contacts[0].geom.g2)!= dRayClass))
      {
        dJointAttach(j, b1, b2);
      }
    }

    //call primitives' notification functions
    if (p1!=NULL && p1->NotifyCollisions)
    {
      p1->OnCollision(numc, contacts, p2);
    }
    if (p2!=NULL && p2->NotifyCollisions)
    {
      p2->OnCollision(numc, contacts, p1);
    }

  }
  //if no collision, nothing to do

}

dSurfaceParameters CWorld::getContact(dGeomID ID1, dGeomID ID2)
{
  const TMaterial *m1,*m2;
  dSurfaceParameters surface;
  
  TGeomData *gd1=(TGeomData*)dGeomGetData(ID1);
  TGeomData *gd2=(TGeomData*)dGeomGetData(ID2);
  
  if (gd1!=NULL && gd1->Primitive!=NULL)
  {
    m1=gd1->Primitive->Material;
  } else {
    m1=&DEFAULT_MATERIAL;
  }

  if (gd2!=NULL && gd2->Primitive!=NULL)
  {
    m2=gd2->Primitive->Material;
  } else {
    m2=&DEFAULT_MATERIAL;
  }
  
  //create contact from our materials
  if (m1==m2) //if materials are the same, no need to calculate surface
  {
    surface=m1->surface;
  } else {
    //just average all, this is how it is apparently done in Webots too ;)
    surface.mode=(m1->surface.mode) | (m2->surface.mode);
    surface.mu=average(m1->surface.mu,m2->surface.mu);
    surface.mu2=average(m1->surface.mu2,m2->surface.mu2);
    surface.bounce=average(m1->surface.bounce,m2->surface.bounce);
    surface.bounce_vel=average(m1->surface.bounce_vel,m2->surface.bounce_vel);
    surface.soft_erp=average(m1->surface.soft_erp,m2->surface.soft_erp);
    surface.soft_cfm=average(m1->surface.soft_cfm,m2->surface.soft_cfm);
    surface.motion1=average(m1->surface.motion1,m2->surface.motion1);
    surface.motion2=average(m1->surface.motion2,m2->surface.motion2);
    surface.slip1=average(m1->surface.slip1,m2->surface.slip1);
    surface.slip2=average(m1->surface.slip2,m2->surface.slip2);
  }
  
  return surface;

}

void CWorld::SetGimiSettingsFile(std::string file)
{
  useGimiSettingsFile=true;
  gimiSettingsFile=file;
}

void CWorld::SetHub(std::string hub, int port)
{
  useGimiSettingsFile=false;
  hubAddress=hub;
  hubPort=port;
}


bool CWorld::ConnectGimi(gimi::GIMI *gimi, std::string name)
{

  int r;
  
  if (useGimiSettingsFile)
  {
    if (!(gimi->readSettings(gimiSettingsFile)))
    { 
      dPrint(1,"Couldn't read connection settings file.");
      return false;
    } else {
      r=gimi->connectToHubEx("",-1,name,true);
    }
  } else {
    r=gimi->connectToHubEx(hubAddress, hubPort, name, true);
  }
  
  
  if (r==GIMI_OK)
  {
    dPrint(1,"Connected to hub %s, port %d using name %s",
        gimi->getHubAddress().c_str(), gimi->getHubPort(), name.c_str());    

    return true;
  } else if (r==GIMI_INVALIDNAME){
    dPrint(1,"Couldn't connect GIMI: Name '%s' already in use.", name.c_str());
    return false;
  } else {
    dPrint(1,"Couldn't connect to hub %s, port%d. GIMI_ERROR.",
            gimi->getHubAddress().c_str(), gimi->getHubPort());
    return false;
  }
}

void CWorld::CreateMapServer()
{
  std::string name = MaCI::GetStaticClientName(iBaseMaCIGroup+"world");
  gimi::GIMI *gimi = new gimi::GIMI();
  //First create gimi and connect to it..
  //Then create MAP SERVER
  MaCI::MaCICtrl::CMaCICtrlServer *msc = new MaCI::MaCICtrl::CMaCICtrlServer(gimi);
  
  using namespace MaCI::Map;
  CMapServer *ms = new CMapServer(msc, 0, "FSRSim Map");  
  iMCS = msc;
  iLineMapServer = ms;

  ms->SetInterfaceInstanceName(std::string("World"));
  // Set macigroup
  std::string macigroup = "Testing";
  msc->SetGroupName(iBaseMaCIGroup);
  iLineMapServerConstructed = true;
  MaCI::EMaCIError e;

  int r;
  
  if (useGimiSettingsFile){
    if (!(gimi->readSettings(gimiSettingsFile))){ 
      dPrint(1,"Couldn't read connection settings file.");
      return ;
    } else {
      r=gimi->connectToHubEx("",-1,name,true);
    }
  } else {
    r=gimi->connectToHubEx(hubAddress,hubPort,name,true);
  }
  
  
  if (r==GIMI_OK){
    dPrint(1,"Connected to hub %s, port %d using name %s",
           gimi->getHubAddress().c_str(), gimi->getHubPort(), name.c_str());    
    if ( (e = ms->Open()) != MaCI::KMaCIOK) {
      dPrint(1,"Failed to Open MapServer instance! (%s)",
             GetErrorStr(e).c_str());
      return;
    } else {
      dPrint(1,"Opened MaCI mapserver");
      
    }
  } else if (r==GIMI_INVALIDNAME){
    dPrint(1,"Couldn't connect GIMI: Name '%s' already in use.", name.c_str());
    return;
  } else {
    dPrint(1,"Couldn't connect to hub %s, port%d. GIMI_ERROR.",
           gimi->getHubAddress().c_str(), gimi->getHubPort());
    return;
  }

}

void CWorld::UpdateMapServer(std::vector<CWall *> walls, bool add)
{
  using namespace MaCI::Map;
  gim::binbag::CBinBag binbag;
  CMapData mapData;
  TMapHeader mapHeader;
  TMapUpdateHeader updateHeader;
 
  float reso = 1.0;

  if(add){
    mapData.EncodeTo(&binbag);
    mapHeader.gridDataResolution=reso;
    if (!mapData.SetMapHeader(mapHeader)){
      
      dPrint(1,"Failed to set map header");
      return;
    }
    
    for(std::vector<CWall *>::size_type i = 0; i < walls.size(); i++){
      TMapLine mapLine;
      mapLine.x1=walls[i]->x1;
      mapLine.y1=walls[i]->y1;
      mapLine.x2=walls[i]->x2;
      mapLine.y2=walls[i]->y2;   
      if (!mapData.AddMapLine(mapLine)) { 
        dPrint(1,"Failed to add line"); 
        return;
      } else { 
        //  dPrint(1,"Added line (%f %f) (%f %f)",walls[i]->x1,walls[i]->y1,walls[i]->x2,walls[i]->y2); 
      }
    }
    
    iLineMapServer->SetMap((const CMapData)mapData);
  }else{
    dPrint(1,"removing???");
    return;
  }  
      

  

}
